#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import EventLog as log
import D_Liner2Joint as D_Liner2Joint_leg
import D_Liner2Joint2 as D_Liner2Joint_hand
import D_R4_LinearMotor_20230922 as D_R4_LinearMotor
import D_R4_RotaryMotor_20230922 as D_R4_RotaryMotor
import D_R4_Finger_20230927 as D_R4_Finger

RAD2DEG = 57.2957786666
# ! 丝杆电机的方向偏转不在这里设置，是直接写在程序中的
JointOpposite = [11,22,31,32,33,35]

JointIDList = [11,12,13,14,15,16
               ,21,22,23,24,25,26
              ,31, 32, 33, 34, 35,36,37
              ,41, 42, 43, 44, 45,46,47
               ,71,72]


class LinkDQ40:
    def __init__(self):
        # self.Namelist = JointsNamesList
        self.RMotor = D_R4_RotaryMotor.RotaryMotorNet()
        self.LMotor = D_R4_LinearMotor.LinearMotorNet()
        self.Finger = D_R4_Finger.Finger()
        self.L2J_EJ = D_Liner2Joint_hand.ElbowJoint()
        self.L2J_WJ = D_Liner2Joint_hand.WristJoint()
        self.L2J_TJ = D_Liner2Joint_leg.ThighJoint()
        self.L2J_KJ = D_Liner2Joint_leg.KneeJoint()
        self.L2J_AJ = D_Liner2Joint_leg.AnkleJoint()

    def SetJointData(self,JointsNameList, JointsSetDataList):
        JointsSendDataList = {}
        for i, jointid in enumerate(JointsNameList):
            if jointid in [11,12,21,22,71,72,31, 32, 33, 35,41, 42, 43, 45]: # 腿部旋转电机,直接输入角度
                if jointid in JointOpposite:
                    JointsSendDataList[jointid] = -JointsSetDataList[i]
                else:
                    JointsSendDataList[jointid] = JointsSetDataList[i]
                if jointid == 22:JointsSendDataList[jointid] = JointsSendDataList[jointid]
            elif jointid in [13,23]:
                JointsSendDataList[jointid] = self.L2J_TJ.angle2lenght(JointsSetDataList[i])
            elif jointid in [14,24]:
                JointsSendDataList[jointid] = self.L2J_KJ.angle2lenght(JointsSetDataList[i])
            elif jointid in [34,44]:
                JointsSendDataList[jointid] = self.L2J_EJ.angle2lenght(JointsSetDataList[i])
            elif jointid == 15: # !必须有16
                # print("JointsNameList", JointsNameList)
                index16 = JointsNameList.index(16)
                JointsSendDataList[15], JointsSendDataList[16] = self.L2J_AJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index16])
            elif jointid == 25:# !必须有26
                index26 = JointsNameList.index(26)
                JointsSendDataList[25],JointsSendDataList[26] = self.L2J_AJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index26])
            elif jointid == 36: # !必须有37
                index37 = JointsNameList.index(37)
                JointsSendDataList[36], JointsSendDataList[37] = self.L2J_WJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index37])
            elif jointid == 46:# !必须有47
                index47 = JointsNameList.index(47)
                JointsSendDataList[46],JointsSendDataList[47] = self.L2J_WJ.angle2lenght(JointsSetDataList[i],JointsSetDataList[index47])
            elif jointid in [16,26]:
                pass
                # ! 在jointid==15,25时已经计算并执行了
            elif jointid in [37,47]:
                pass
                # ! 在jointid==37,47时已经计算并执行了
            elif jointid in [51,52,53,54,55,56,61,62,63,64,65,66]:
                JointsSendDataList[jointid] = self.Normalized(jointid,JointsSetDataList[i])
            else:
                pass
        # print(JointsSendData)
        SendFingerL = []
        SendFingerR = []
        
        
        for jointid in JointsSendDataList:
            # print(jointid,JointsSendData[jointid])
            if jointid in [11,12,21,22,71,72,31, 32, 33, 35,41, 42, 43, 45]:
                # 直线电机最大速度15000，加速度200
                self.RMotor.SetMotorData(jointid,JointsSendDataList[jointid],7000, 200)
            elif jointid in [13,14,23,24,34,44,36,37,46,47]:
                # 直线电机最大速度20000，加速度200
                self.LMotor.SetMotorData(jointid,JointsSendDataList[jointid],10000, 150)
                # print("JointsSendDataList",JointsSendDataList)
            elif jointid in [15,16,25,26]:
                # 直线电机最大速度20000，加速度200
                self.LMotor.SetMotorData(jointid,JointsSendDataList[jointid],20000, 200)
                # print("JointsSendDataList",JointsSendDataList)
            elif jointid in [51,52,53,54,55,56]:
                SendFingerL.append([jointid,JointsSendDataList[jointid]])
            elif jointid in [61,62,63,64,65,66]:
                SendFingerR.append([jointid,JointsSendDataList[jointid]])
            else:
                self.LMotor.SetMotorData(jointid, JointsSendDataList[jointid], 5000, 50)
        
        print(SendFingerR,SendFingerL)
        # 手指电机，6个为一组，一起发送
        if len(SendFingerL)==6:
            self.Finger.SetARMMulData(SendFingerL)
        if len(SendFingerR)==6:
            self.Finger.SetARMMulData(SendFingerR)
            
                
    def GetJointData(self, JointsNameList):
        ret = []
        rlt = []

        for i, jointid in enumerate(JointsNameList):
            if jointid in [11,12,21,22,71,72,31,32,33,35,41,42,43,45]:
                r = self.RMotor.GetMotorData(jointid)
                ret.append(r[0])
                if jointid in JointOpposite:
                    rlt.append(-r[1])
                else:
                    rlt.append(r[1])
            elif jointid in [13,23]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_TJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [14,24]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_KJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [15,25]:
                r5 = self.LMotor.GetMotorData(jointid)
                r6 = self.LMotor.GetMotorData(jointid+1) # !读16or26电机
                # print("L15,L16",jointid,r5,r6)
                ret.append(min(r5[0],r6[0]))
                r = self.L2J_AJ.lenght2angle(r5[1],r6[1])
                rlt.append(round(r[0],2))
            elif jointid in [16,26]:
                r5 = self.LMotor.GetMotorData(jointid-1) # !读15or25电机
                r6 = self.LMotor.GetMotorData(jointid)
                ret.append(min(r5[0],r6[0]))
                r = self.L2J_AJ.lenght2angle(r5[1],r6[1])
                rlt.append(round(r[1],2))
            elif jointid in [34,44]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_EJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [36,46]:
                r6 = self.LMotor.GetMotorData(jointid)
                r7 = self.LMotor.GetMotorData(jointid+1) # !读36or37电机
                ret.append(min(r6[0],r7[0]))
                r = self.L2J_WJ.lenght2angle(r6[1],r7[1])
                rlt.append(round(r[0],2))
            elif jointid in [37,47]:
                r6 = self.LMotor.GetMotorData(jointid-1)
                r7 = self.LMotor.GetMotorData(jointid) # !读46or47电机
                ret.append(min(r6[0],r7[0]))
                r = self.L2J_WJ.lenght2angle(r6[1],r7[1])
                if jointid == 47:
                    rlt.append(round(-r[1],2))
                else:
                    rlt.append(round(r[1],2))
            else:
                ret.append(0)
                rlt.append(0)

        return ret,rlt

    def Normalized(self,id, indata):
        # 51-56为左手手指，61-66为右手手指
        # 51/61:大拇指上下移动，范围上限：1700 ，范围下限：600
        # 56/66:大拇指左右移动，范围上限：1700 ，范围下限：150 # 方向相反
        # 52/62:食指，范围上限：1800 ，范围下限：700
        # 53/63:中指，范围上限：1800 ，范围下限：700
        # 54/64:无名指，范围上限：1800 ，范围下限：700
        # 55/65:小拇指，范围上限：1800 ，范围下限：700

        outdata = -100
        if 0 <= indata <= 100:
            if id in [52,53,54,55,62,63,64,65]:
                outdata = int((indata / 100 * (1800 - 750)) + 750)
            elif id in [56, 66]:# 为了防止手指锁死，这两个电机这里做了限制
                outdata = int(((100-indata) / 100 * (1002 - 150)) + 150)
            elif id in [51, 61]:
                outdata = int((indata / 100 * (1700 - 600)) + 600)
            else:
                pass

        return outdata

    def __del__(self):
        pass

if __name__ == "__main__":
    LinkDQ40 = LinkDQ40()
    # ret,rlt = LinkDQ40.GetJointData(JointIDList)
    # print(ret,rlt)
    # print("ret = ",dict(zip(JointIDList,ret)))
    # print("rlt = ",dict(zip(JointIDList,rlt)))
    # time.sleep(1)
    
    while True:
        for i in range(0,30,1):
            t = i*0.3
            JointList = [51,52,53,54,55,56]
            LinkDQ40.SetJointData(JointList,[10*t,10*t,10*t,10*t,10*t,10*t])
            # time.sleep(0.001)
            
            JointList = [61,62,63,64,65,66]
            LinkDQ40.SetJointData(JointList,[10*t,10*t,10*t,10*t,10*t,10*t])
            time.sleep(0.001)
            
        for i in range(30,0,-1):
            t = i*0.3
            JointList = [51,52,53,54,55,56]
            LinkDQ40.SetJointData(JointList,[10*t,10*t,10*t,10*t,10*t,10*t])
            # time.sleep(0.05)
            
            JointList = [61,62,63,64,65,66]
            LinkDQ40.SetJointData(JointList,[10*t,10*t,10*t,10*t,10*t,10*t])
            time.sleep(0.001)
        
    # LinkDQ40.SetJointData(JointList,[0])
    # time.sleep(1)
    
    # print(LinkDQ40.GetJointData(JointList))
    # time.sleep(1)
    
    # JointList = [31,32,33,35,41,42,43,45,34,44]
    # LinkDQ40.SetJointData(JointList,[15,15,15,15,15,15,15,15,40,40])
    # time.sleep(1)
    # LinkDQ40.SetJointData(JointList,[0,0,0,0,0,0,0,0,0,0,15,15])
    # time.sleep(2)
    # print(LinkDQ40.GetJointData([34, 44]))
    # time.sleep(1)
    
    # JointList = [36,37]
    # LinkDQ40.SetJointData(JointList,[10,10])
    # time.sleep(1)
    # LinkDQ40.SetJointData(JointList,[10,10,84,84])
    # time.sleep(2)
    # print(LinkDQ40.GetJointData([32,42,34, 44]))
    # time.sleep(1)

    # 单独测试丝杆电机算法
    # LMotor = D_Liner2Joint.AnkleJoint()
    # print(LMotor.angle2lenght(6.63,0))
    # print(LMotor.lenght2angle(297.84,297.84))
    #

    # print(LinkDQ40.GetJointData([25, 26]))

    # JointsNameList = [15, 16,25,26]
    #
    # JointsDataList = [0,0,0,0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)
    #
    # JointsDataList = [30,-15,30,-15]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(2)
    #
    #
    # time.sleep(2)
    # print(LinkDQ40.GetJointData(JointsNameList))
    #
    # JointsDataList = [0,0,0,0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)

    # JointsNameList = [12,13,14,15, 16,25,26]
    #
    # JointsDataList = [0,0,0,0,0,0,0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)

    # JointsNameList = [11, 12, 21, 22, 71, 72]
    # JointsDataList = [ 0, 0, 0, 0, 0, 0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)
    #
    # JointsNameList = [13,23,14,24,15,25,16,26]
    # JointsDataList = [5,5,20,20,15,15,0,0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)

    # JointsNameList = [25]
    # JointsDataList = [0]
    # print(LinkDQ40.GetJointData(JointsNameList))
    # time.sleep(1)

    # JointsNameList = [22]
    # JointsDataList = [5]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    # time.sleep(1)
    # JointsNameList = [12, 22]

    # JointsDataList = [0, 0]
    # LinkDQ40.SetJointData(JointsNameList, JointsDataList)
    #
    # time.sleep(1)
